#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <ceres/ceres.h>

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;

using namespace std;
using namespace cv;


void findmatch(const Mat &img1, const Mat &img2, vector<KeyPoint> &keypoints_1, 
               vector<KeyPoint> &keypoints_2, vector<DMatch> &goodmatches)
{
    Mat descriptors1, descriptors2;
    Ptr<FeatureDetector> detector=ORB::create();
    Ptr<DescriptorExtractor> descriptor=ORB::create();
    Ptr<DescriptorMatcher> matcher =DescriptorMatcher::create("BruteForce-Hamming");
    
    detector->detect(img1, keypoints_1);
    detector->detect(img2, keypoints_2);
    
    descriptor->compute(img1, keypoints_1, descriptors1);
    descriptor->compute(img2, keypoints_2, descriptors2);
    
    matcher->match(descriptors1, descriptors2, goodmatches);
}

void getdateset(const vector<DMatch> &goodmatches, const vector<KeyPoint> &keypoint1, 
                const  vector<KeyPoint> &keypoint2, double *x1, double *y1, double *x2, 
                double *y2)
{
    for(int i=0; i<goodmatches.size();i++){
        *(x1+i)=keypoint1[goodmatches[i].queryIdx].pt.x;
        *(y1+i)=keypoint1[goodmatches[i].queryIdx].pt.y;
        *(x2+i)=keypoint2[goodmatches[i].queryIdx].pt.x;
        *(y2+i)=keypoint2[goodmatches[i].queryIdx].pt.y;
    }      
}

struct affineResidual{
    affineResidual(double x1, double y1, double x2, double y2):x1_(x1), y1_(y1), x2_(x2), y2_(y2){}
    template <typename T>
    bool operator() (const T *const m, T* residual) const{
    T a1=*(m+0)*x1_+*(m+1)*y1_+*(m+2);
    T a2=*(m+3)*x1_+*(m+4)*y1_+*(m+5);
    residual[0]=x2_-a1;
    residual[1]=y2_-a2;
    return true;
    }
private:
    const double x1_, y1_,x2_,  y2_;
};

void dataopt(const double * x1, const double * y1, 
             const double * x2, const double * y2,
             const int &numpoint, const int & numite, double *m){
    Problem problem;
    for(int i=0; i<numpoint; ++i){
    problem.AddResidualBlock(
        new AutoDiffCostFunction<affineResidual,2,6>(
            new affineResidual(*(x1+i),*(y1+i),*(x2+i),*(y2+i))
                                                  ),
                             NULL,
                             m
            );
    }
    
    Solver::Options options;
    options.max_num_iterations=numite;
    options.linear_solver_type=ceres::DENSE_QR;
    options.minimizer_progress_to_stdout=true;
    
    Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    std::cout<<summary.BriefReport()<<"\n";
    std::cout<<*(m+0)<<" "<<*(m+1)<<" "<<*(m+2)<<"\n";
    std::cout<<*(m+3)<<" "<<*(m+4)<<" "<<*(m+5)<<"\n";
}

int main(int argc, char **argv) {
    
    Mat image1=imread("../123.jpeg", IMREAD_ANYCOLOR);
    Mat image2=imread("../234.jpeg", IMREAD_ANYCOLOR);
    assert(image1.data!=nullptr&&image2.data!=nullptr);
    
    vector<KeyPoint> keypoint1, keypoint2;
    vector<DMatch> goodmatches;
    
    findmatch(image1, image2, keypoint1, keypoint2, goodmatches);
    Point2f srcpt1, srcpt2, srcpt3, dstpt1, dstpt2, dstpt3;
    srcpt1.x=keypoint1[goodmatches[1].queryIdx].pt.x;
    srcpt1.y=keypoint1[goodmatches[1].queryIdx].pt.y;
    dstpt1.x=keypoint2[goodmatches[1].trainIdx].pt.x;
    dstpt1.y=keypoint2[goodmatches[1].trainIdx].pt.y;
    
    srcpt2.x=keypoint1[goodmatches[5].queryIdx].pt.x;
    srcpt2.y=keypoint1[goodmatches[5].queryIdx].pt.y;
    dstpt2.x=keypoint2[goodmatches[5].trainIdx].pt.x;
    dstpt2.y=keypoint2[goodmatches[5].trainIdx].pt.y;
    
    srcpt3.x=keypoint1[goodmatches[19].queryIdx].pt.x;
    srcpt3.y=keypoint1[goodmatches[19].queryIdx].pt.y;
    dstpt3.x=keypoint2[goodmatches[19].trainIdx].pt.x;
    dstpt3.y=keypoint2[goodmatches[19].trainIdx].pt.y;
     
    Point2f src[]={srcpt1,srcpt2,srcpt3};
    Point2f dst[]={dstpt1,dstpt2,dstpt3};
    Mat affine;
    affine=getAffineTransform(src,dst);
    cout<<affine<<endl; 
    
    double a00=0,a01=0,a02=0,a10=0,a11=0,a12=0;
    double m[6]={a00,a01,a02,a10,a11,a12};
    double x1[goodmatches.size()],y1[goodmatches.size()],x2[goodmatches.size()],y2[goodmatches.size()];
    getdateset(goodmatches,keypoint1,keypoint2,x1,y1,x2,y2);
    
/***
    double x1[3],y1[3],x2[3],y2[3];
    x1[0]=srcpt1.x;
    x1[1]=srcpt2.x;
    x1[2]=srcpt3.x;
    y1[0]=srcpt1.y;
    y1[1]=srcpt2.y;
    y1[2]=srcpt3.y;
    x2[0]=dstpt1.x;
    x2[1]=dstpt2.x;
    x2[2]=dstpt3.x;
    y2[0]=dstpt1.y;
    y2[1]=dstpt2.y;
    y2[2]=dstpt3.y;
***/

    dataopt(x1,y1,x2,y2,500,30,m);
    return 0;
}
